{# ----------------------------------------------------------------------------
 # SymForce - Copyright 2022, Skydio, Inc.
 # This source code is under the Apache 2.0 license found in the LICENSE file.
 # ---------------------------------------------------------------------------- #}

/* ----------------------------------------------------------------------------
 * SymForce - Copyright 2022, Skydio, Inc.
 * This source code is under the Apache 2.0 license found in the LICENSE file.
 * ---------------------------------------------------------------------------- */

/**
 * Tests for generated functions that accept C++ camera types as arguments. Mostly testing that
 * everything compiles.
 */

#include <Eigen/Core>

#include <catch2/catch_template_test_macros.hpp>
#include <catch2/catch_test_macros.hpp>

// TODO(nathan): We just test linear camera for now, but could/should test other types in the future
#include <sym/linear_camera_cal.h>
#include "symforce_function_codegen_test_data/symengine/symforce_gen_codegen_test_data/cam_function_codegen_test/pixel_to_ray_and_back.h"

TEMPLATE_TEST_CASE("Test generated function", "[cam_function]", sym::LinearCameraCal<double>,
                   sym::LinearCameraCal<float>) {
  using T = TestType;
  using Scalar = typename T::Scalar;
  Scalar epsilon = 1e-6; // For preventing degenerate numerical cases (e.g. division by zero)

  Eigen::Matrix<Scalar, sym::StorageOps<T>::StorageDim(), 1> data;
  std::mt19937 gen(42);
  std::uniform_real_distribution<Scalar> cam_dist(100.0, 500.0);
  for (int i = 0; i < sym::StorageOps<T>::StorageDim(); i++) {
    data[i] = cam_dist(gen);
  }
  T cam(data);
  INFO("Testing generated function with " << cam);

  Eigen::Matrix<Scalar, 2, 1> pixel;
  pixel << 2.0 * cam_dist(gen), 2.0 * cam_dist(gen);
  Eigen::Matrix<Scalar, 2, 1> pixel_reprojected =
      cam_function_codegen_test::PixelToRayAndBack<Scalar>(pixel, cam, epsilon);
  CHECK(pixel.isApprox(pixel_reprojected, epsilon));
}
